/*
 *  BodyJoint.m
 *  pogostick
 *
 *  Created by daddy (chuck@kollar.com) on 9/4/09.
 *  Copyright 2009 Charles P. Kollar All rights reserved.
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#import "BodyJoint.h"

// The amount (increment) by which a joint is moved if it is told to...
#define DELTA                               (M_PI/200.0f)


@implementation BodyJoint

-(id) initInSpace:(cpSpace *)sp trunkBody:(cpBody *)tb armBody:(cpBody *)ab right:(int) __attribute__ ((unused)) r
{
	if (self = [super init])
    {
		joint = (cpGearJoint *)cpGearJointNew(tb, ab, 0.0f, 1.0f);
		//rSholderJoint->maxForce = MAX_JOINT_FORCE;
		cpSpaceAddConstraint(sp, (cpConstraint *)joint);
		jointDelta = 0.0f;
	}
	return self;
}

-(void) setJointMaxForce:(cpFloat)mf
{
	((cpConstraint *)joint)->maxForce = mf;
}

-(void) rotate:(cpFloat)amountAngle{
	jointDelta = DELTA * (amountAngle < 0.0f ? -1.0f : 1.0f);
	destinationAngle = joint->phase + amountAngle;
	distance = cpvdot(cpv(cos(joint->phase),sin(joint->phase)), cpv(cos(destinationAngle), sin(destinationAngle)));
#if 0
	NSLog(@"BodyJoint:rotate jointDelta %f joint->phase %f destinationAngle %f distance %f\n",
          jointDelta, joint->phase, destinationAngle, distance);
#endif
}

-(void) move
{
	if (jointDelta == 0.0f) return;

	cpFloat nextPhase = joint->phase + jointDelta;
	cpFloat nextDistance = cpvdot(cpv(cos(nextPhase), sin(nextPhase)), cpv(cos(destinationAngle), sin(destinationAngle)));
    // fabs doesn't work here?!
	if (abs(nextDistance) > abs(distance)) jointDelta = 0.0f;
#if 0
	NSLog(@"BodyJoint:move jointDelta %f joint->phase %f destinationAngle %f distance %f nextDistance %f\n",
          jointDelta, joint->phase, destinationAngle, distance, nextDistance);
#endif
	distance = nextDistance;
	joint->phase = nextPhase;
}

-(void) dealloc {
	//cpConstraintFree((cpConstraint *)joint);
    
    [super dealloc];
}

@end
